#include "mot.h"
#include <string.h>
#include <stdio.h>

extern __IO uint32_t get_tim3_ms();
extern __IO uint32_t get_tim3_us();

extern TIM_HandleTypeDef htim2;
//TIM_HandleTypeDef htim3;


struct MotEncoder ChassisMot[MOT_NUM];

void mot_init()
{
	//硬件初始化已由cube配置完成
	//初始化定时器T2 内部时钟， PWM1模式输出， 2分频， ARR=2000

	//启动定时器
	memset(ChassisMot, 0, sizeof(struct MotEncoder)*MOT_NUM);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
	HAL_TIM_Base_Start(&htim2);

	TIM2->PSC = 1;
	TIM2->ARR = 2000;
	TIM2->CCR1=0;
	TIM2->CCR2=0;
	TIM2->CCR3=0;
	TIM2->CCR4=0;

	// 首先拉低使能脚；然后设置电机驱动器电平，再拉高使能脚（参考DRV8848PWPR datesheet）
	HAL_GPIO_WritePin(MOTEN1_GPIO_Port, MOTEN1_Pin, GPIO_PIN_RESET);
	HAL_Delay(2);
	HAL_GPIO_WritePin(MOTEN1_GPIO_Port, MOTEN1_Pin, GPIO_PIN_SET);
}

void mot_proc(void* arg) {
	uint32_t cur = get_tim3_ms();
	uint32_t cur_us = get_tim3_us();

	for(int i=0; i<MOT_NUM; ++i) {
		ChassisMot[i].timepoint = cur; //使用系统时钟
		if(cur - ChassisMot[i].wd_ms > MOT_TIMEOUT){ //判断是否超时
			ChassisMot[i].dir = 0;
			ChassisMot[i].rpm = 0.0;
			ChassisMot[i].wd_ms = cur;
			ChassisMot[i].wd_us = cur_us;
			ChassisMot[i].wd_idx = 0;
			memset(ChassisMot[i].pulse_time, 0, sizeof(uint32_t)*MOT_FILTER_FACTOR);
//			printf("Mot calc rmp timeout\r\n");
		}
		else {
			uint32_t total = 0;
			for(int j=0; j<MOT_FILTER_FACTOR; j++) {
				total += ChassisMot[i].pulse_time[j];
			}
			if(total == 0) {
				ChassisMot[i].rpm = 0.0;
//				printf("Mot calc rmp totol = 0\r\n");
			}
			else {
//				printf("Mot calc rmp success\r\n");
				ChassisMot[i].rpm = (60000000.0 * MOT_FILTER_FACTOR) / (total  * MOT_REDUCTION_FACTOR * MOT_PULSE_COUNT);
				ChassisMot[i].rpm = (ChassisMot[i].dir==DIR_FORWARD ? ChassisMot[i].rpm : -ChassisMot[i].rpm);
			}
		}
	}
}

int mot_get(uint8_t no, double* rpm, uint32_t* timepoint)
{
	if(no<0 || no>=MOT_NUM) {
		return -1;
	}
	*rpm = ChassisMot[no].rpm;
	*timepoint = ChassisMot[no].timepoint;
	return 0;
}

// 获取电机编码器raw信息
int mot_get_rawinfo(uint8_t no, struct MotEncoder** praw)
{
	if(no<0 || no>=MOT_NUM) {
		return -1;
	}

	*praw = &ChassisMot[no];
	return 0;
}

int mot_set(uint8_t no, int percent)
{
	int ret = -1;
	switch(no){
	case 0:
		if(percent>20) {
			TIM2->CCR1=percent+500;	TIM2->CCR2=0;
		}
		else if(percent<-20) {
			percent=-percent;
			TIM2->CCR1=0;		TIM2->CCR2=percent+500;  //25%启动
		}
		else {
			TIM2->CCR1=0;		TIM2->CCR2=0;
		}
		ret = 0;
		break;
	case 1:
		if(percent > 20) {
			TIM2->CCR3=percent+500;		TIM2->CCR4=0;
		}
		else if(percent<-20) {
			percent=-percent;
			TIM2->CCR3=0;	TIM2->CCR4=percent+500;
		}
		else {
			TIM2->CCR3=0;		TIM2->CCR4=0;
		}
		ret = 0;
		break;
	default:
		break;
	}

    return ret;
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	uint32_t cur_ms = get_tim3_ms();
	uint32_t cur_us = get_tim3_us();
	switch(GPIO_Pin) {
	case GPIO_PIN_3:
		ChassisMot[0].pulse_time[ChassisMot[0].wd_idx++] =
				(cur_ms - ChassisMot[0].wd_ms)*1000 + (cur_us-ChassisMot[0].wd_us);
		ChassisMot[0].wd_ms = cur_ms;
		ChassisMot[0].wd_us = cur_us;
		if(ChassisMot[0].wd_idx == MOT_FILTER_FACTOR) {
			ChassisMot[0].wd_idx = 0;
		}
		ChassisMot[0].dir =
				(HAL_GPIO_ReadPin(HALLA2_GPIO_Port, HALLA2_Pin) == GPIO_PIN_SET ? DIR_FORWARD : DIR_REVERSE);
		break;
	case GPIO_PIN_4:
		ChassisMot[1].pulse_time[ChassisMot[1].wd_idx++] =
				(cur_ms - ChassisMot[1].wd_ms)*1000 + (cur_us-ChassisMot[1].wd_us);
		ChassisMot[1].wd_ms = cur_ms;
		ChassisMot[1].wd_us = cur_us;
		if(ChassisMot[1].wd_idx == MOT_FILTER_FACTOR) {
			ChassisMot[1].wd_idx = 0;
		}
		ChassisMot[1].dir =
				(HAL_GPIO_ReadPin(HALLB2_GPIO_Port, HALLB2_Pin) == GPIO_PIN_RESET ? DIR_FORWARD : DIR_REVERSE);
		break;
	case GPIO_PIN_5:
		break;
	case GPIO_PIN_11:
		break;
	default:
		break;
	}
}

void mot_dump()
{
	for(int i=0; i<MOT_NUM; ++i) {
	  int rpm = (int)ChassisMot[i].rpm;
	  printf("-----M[%d], RPM=%d raw=[%d] [%d] [%d] [%d].\r\n",
			  i, rpm, ChassisMot[i].pulse_time[0],ChassisMot[i].pulse_time[1],
			  ChassisMot[i].pulse_time[2],ChassisMot[i].pulse_time[3]);
	}
}
